/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Robo;

/**
 *
 * @author paul
 */

public class RoboCopilot extends RoboTask {
    double lastTime = 0;
    double targetAngle = 0;
    final double maxRotSpeed = 360; // deg/second
    boolean autoTurnActive = true;

    public void start(){
        super.start();
    }

    public void handle(){
        if(!active) return;
        super.handle();
        double dt = 0.0;
        if(lastTime > 0){
            dt = taskTime - lastTime;
            lastTime = taskTime;
        }

        // robot.drive.go(-leftDriveStick.getY(), leftDriveStick.getX(), rightDriveStick.getX());

        // read driver controls
        double x = robot.driverStation.leftDriveStick.getX();
        double y = -robot.driverStation.leftDriveStick.getY();
        double r = robot.driverStation.rightDriveStick.getX();

        // check if in tracking mode and line visible
        if(robot.driverStation.leftDriveStick.getRawButton(1) && robot.lineTracker.lineVisible){
            // auto-approach
            // ...
        }
        else {
            // manual driving
            robot.drive.ySpeed = y;
            robot.drive.xSpeed = x;
        }

        if(robot.driverStation.leftDriveStick.getRawButton(8)){
            // gyro off
            autoTurnActive = false;
        }
        if(robot.driverStation.leftDriveStick.getRawButton(9)){
            // gyro on - reset
            targetAngle = robot.drive.readGyro();
            autoTurnActive = true;
        }

        if(autoTurnActive){
            // adjust target angle based on selected rotational speed
            targetAngle += (dt * maxRotSpeed * r);

            // adjust rotation speed based on target vs. actual angle
            if(true){
                double da = targetAngle - robot.drive.readGyro();
                double dabs = Math.abs(da);
                if(dabs < 2) {
                    robot.drive.rotSpeed = 0;
                }
                else if(dabs < 15){
                    robot.drive.rotSpeed = da / 15;
                }
                else {
                    robot.drive.rotSpeed = signum(da);
                }
            }
        }
        else {
            // just apply turn signal directly
            robot.drive.rotSpeed = r;
        }
    }
}
